/// 计算Dicom图的小钢球，并且知道小钢球的实际位置，计算相机参数
/// TODO: 结果无法对应上，开始计算的P 第三行 角度值不对
#include <Eigen/Core>

#include "opencv2/imgcodecs.hpp"
#include <opencv2/imgproc.hpp>
#include "opencv2/highgui.hpp"

#include <vtkDICOMImageReader.h>
#include <vtkImageData.h>

#include <memath/MeVtkMath.h>
#include <memath/meTransform.h>
#include "CeresSolProjMatrix.h"

#include "algorithm/dicomdetect/Parse2DDicomImage.h"

using namespace std;
using namespace cv;


double distance(const KeyPoint& p1, const KeyPoint& p2)
{
    return sqrt(pow(p1.pt.x - p2.pt.x, 2) + pow(p1.pt.y - p2.pt.y, 2));
}


/// imageData 传递一层图像， 二值化图像
bool dicomToBinaryMat(vtkImageData* imageData, cv::Mat &binayMat, short thresholdMin, short thresholdMax, bool toLog)
{
    int dims[3];
    imageData->GetDimensions(dims);
    cout << "dims:" << dims[0] << ", " << dims[1] << ", " << dims[2] << endl;
    double scalarRange[2];
    imageData->GetScalarRange(scalarRange);
    if (toLog) {
        int scalarSize = imageData->GetScalarSize();
        int scalarType = imageData->GetScalarType();
        double typeMax = imageData->GetScalarTypeMax();
        double typeMin = imageData->GetScalarTypeMin();
        cout << "scalarSize:" << scalarSize << "scalarType:" << scalarType << "typeMin:" << typeMin << "typeMax:" << typeMax << endl;
        cout << "scalarRange:" << scalarRange[0] << scalarRange[1] << endl;
        cout << "scalar Type:" << imageData->GetScalarTypeAsString() << endl;
    }

    ushort *pData = static_cast<ushort*>(imageData->GetScalarPointer());
    ushort minValue = ushort(scalarRange[0]), maxValue = ushort(scalarRange[1]);

    // 返回一个RGBA的QImage用于显示，以及一个灰色gray的Mat用于获取铁球的位置
    ushort range = maxValue - minValue;
    // 有时maxValue == minValue, 此时range应该为1
    if (range == 0) range = 1;
    binayMat.create(dims[1], dims[0], CV_8UC1);

    for (int y = 0; y < dims[1]; y++) {
        uchar *matLine = binayMat.ptr(y);
        int loffset = dims[0] * (dims[1] - 1 - y);
        for (int x = 0; x < dims[0]; x++) {
            short value = pData[loffset + x];
            if (value > thresholdMin && value < thresholdMax) {
                matLine[x] = 0;
            } else {
                matLine[x] = 255;
            }
        }
    }

    return true;
}

/// imageData 传递一层图像， 二值化图像
bool dicomToGrayMat(vtkImageData* imageData, cv::Mat &grayMat, short thresholdMin, short thresholdMax, bool toLog)
{
    int dims[3];
    imageData->GetDimensions(dims);
    double scalarRange[2];
    imageData->GetScalarRange(scalarRange);

    ushort *pData = static_cast<ushort*>(imageData->GetScalarPointer());
    ushort minValue = ushort(scalarRange[0]), maxValue = ushort(scalarRange[1]);

    // 返回一个RGBA的QImage用于显示，以及一个灰色gray的Mat用于获取铁球的位置
    ushort range = maxValue - minValue;
    // 有时maxValue == minValue, 此时range应该为1
    if (range == 0) range = 1;
    grayMat.create(dims[1], dims[0], CV_8UC1);

    for (int y = 0; y < dims[1]; y++) {
        uchar *matLine = grayMat.ptr(y);
        int loffset = dims[0] * (dims[1] - 1 - y);
        for (int x = 0; x < dims[0]; x++) {
            short value = pData[loffset + x];
            if (value < thresholdMin) {
                matLine[x] = 0;
            } else if (value > thresholdMax){
                matLine[x] = 255;
            } else {
                matLine[x] = (value - thresholdMin)/(thresholdMax-thresholdMin);
            }
        }
    }

    return true;
}

/// 读取中心点
bool findImageCircles(vtkImageData *imageData, double threshold, double threshold2,
                      cv::Mat &binaryMat, std::vector < std::vector<cv::Point> > &contours, std::vector<cv::KeyPoint> &centers)
{
    dicomToBinaryMat(imageData, binaryMat, threshold, threshold2, true);
    findContours(binaryMat, contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);

    centers.resize(0);
    for (std::vector< std::vector<cv::Point>>::iterator it = contours.begin(); it != contours.end(); ) {
        if (it->size() < 10) {
            contours.erase(it);
            continue;
        }
        cv::KeyPoint center;
        Moments moms = moments(*it);
        if(moms.m00 == 0.0) {
            contours.erase(it);
            continue;
        }
        double area = moms.m00;
        if (area < 100 || area >= 800) {
            //qDebug("Skip for area %f is not expected.", area);
            contours.erase(it);
            continue;
        }

        double perimeter = arcLength(*it, true);
        double ratio = 4 * CV_PI * area / (perimeter * perimeter);
        if (ratio < 0.6) {
            //qDebug("Skip for circularity (%f) is not expected", ratio);
            contours.erase(it);
            continue;
        }

        center.pt.x =moms.m10 / moms.m00;
        center.pt.y = moms.m01 / moms.m00;

        //compute blob radius
        {
            // std::vector<double> dists;
            double dSum = 0;
            int minx=INT_MAX, miny=INT_MAX, maxx=0, maxy=0;
            for (size_t pointIdx = 0; pointIdx < it->size(); pointIdx++)
            {
                Point2f pt = (*it)[pointIdx];
                //dists.push_back(norm(center.location - pt));
                dSum += norm(center.pt - pt);
            }
            center.size = 2*dSum/it->size();
        }

        centers.push_back(center);

        it++;
    }
    return true;
}

/// 读取dicom并识别出全部小球（以前使用opencv二值化可能有问题）
/// 通过调整 两个阈值 来看效果
bool readDicomToBinaryImage(std::string& filename, bool toLog)
{
    vtkSmartPointer<vtkDICOMImageReader> reader = vtkSmartPointer<vtkDICOMImageReader>::New();
    if (!reader->CanReadFile(filename.c_str())) {
        std::cout << "[getDicomImageData] Unable to read file:" << filename << endl;;
        return false;
    }

    reader->SetFileName(filename.c_str());
    reader->Update();

    // 图像的基本信息
    int dims[3];
    reader->GetOutput()->GetDimensions(dims);

    vtkImageData* imageData = reader->GetOutput();
    double scalarRange[2];
    imageData->GetScalarRange(scalarRange);
    if (toLog) {
        int scalarSize = imageData->GetScalarSize();
        int scalarType = imageData->GetScalarType();
        double typeMax = imageData->GetScalarTypeMax();
        double typeMin = imageData->GetScalarTypeMin();
        cout << "scalarSize:" << scalarSize << "scalarType:" << scalarType << "typeMin:" << typeMin << "typeMax:" << typeMax << endl;
        cout << "scalarRange:" << scalarRange[0] << scalarRange[1] << endl;
        cout << "scalar Type:" << imageData->GetScalarTypeAsString() << endl;
    }

    ushort *pData = static_cast<ushort*>(imageData->GetScalarPointer());
    ushort minValue = ushort(scalarRange[0]), maxValue = ushort(scalarRange[1]);

    // 返回一个RGBA的QImage用于显示，以及一个灰色gray的Mat用于获取铁球的位置
    ushort range = maxValue - minValue;
    // 有时maxValue == minValue, 此时range应该为1

    string mat1Str = "./irondect/mat1.png";

    double threshold = 13200;
    double threshold2 = 23500;
    cv::namedWindow("binaryimage", 0);
    cv::resizeWindow("binaryimage", 1000, 1000);
    std::vector<KeyPoint> centers;
    while (true) {
        cv::Mat mat1;
        std::vector < std::vector<cv::Point> > contours;
        centers.resize(0);

        cout << "threshold = " << threshold << " threshold2 = " << threshold2 << endl;
        findImageCircles(imageData, threshold, threshold2, mat1, contours, centers);

        Mat draw;
        cvtColor(mat1, draw, COLOR_GRAY2BGR);
        Scalar color = Scalar(255, 0, 0);
        drawContours(draw, contours, -1, color, 3, 8);

        cout << "please press: left, right, o, p, q" << endl;
        cv::imshow("binaryimage", draw);

        int key = cv::waitKey();
        cout << "key=" << key << endl;
        if (key == 'q') {
            break;
        } else if (key == 81) { // left
            threshold -= 100;
        } else if (key == 83) { // right
            threshold += 100;
        } else if (key == 'o') {
            threshold2 -= 100;
        } else if (key == 'p') {
            threshold2 += 100;
        }
    }

    return true;
}

/// 读取dicom并识别出全部小球（以前使用opencv二值化可能有问题）
/// 通过固定的阈值
bool findDicomIron(std::string& filename, QString *imageOutputFolder,
                   vector<cv::Point2f> &innerPts, vector<cv::Point2f> &outerPts,
                   bool bDebug)
{
    vtkSmartPointer<vtkDICOMImageReader> reader = vtkSmartPointer<vtkDICOMImageReader>::New();
    if (!reader->CanReadFile(filename.c_str())) {
        std::cout << "[getDicomImageData] Unable to read file:" << filename << endl;;
        return false;
    }

    reader->SetFileName(filename.c_str());
    reader->Update();

    // 图像的基本信息
    int dims[3];
    reader->GetOutput()->GetDimensions(dims);

    vtkImageData* imageData = reader->GetOutput();
    double scalarRange[2];
    imageData->GetScalarRange(scalarRange);
    if (bDebug) {
        int scalarSize = imageData->GetScalarSize();
        int scalarType = imageData->GetScalarType();
        double typeMax = imageData->GetScalarTypeMax();
        double typeMin = imageData->GetScalarTypeMin();
        cout << "scalarSize:" << scalarSize << "scalarType:" << scalarType << "typeMin:" << typeMin << "typeMax:" << typeMax << endl;
        cout << "scalarRange:" << scalarRange[0] << scalarRange[1] << endl;
        cout << "scalar Type:" << imageData->GetScalarTypeAsString() << endl;
    }

    ushort *pData = static_cast<ushort*>(imageData->GetScalarPointer());
    ushort minValue = ushort(scalarRange[0]), maxValue = ushort(scalarRange[1]);

    // 返回一个RGBA的QImage用于显示，以及一个灰色gray的Mat用于获取铁球的位置
    ushort range = maxValue - minValue;
    // 有时maxValue == minValue, 此时range应该为1

    string mat1Str = "./irondect/mat1.png";

    vector<vector<double>> thresholds = {
        {13200, 23500},
        {20000, 25000},
    };

    cv::namedWindow("binaryimage", 0);
    cv::resizeWindow("binaryimage", 1000, 1000);
    std::vector<KeyPoint> centers;
    for (int i=0; i< thresholds.size(); i++) {

        cv::Mat mat1;
        std::vector < std::vector<cv::Point> > contours;
        std::vector<KeyPoint> subcenters;

        cout << "threshold = " << thresholds[i][0] << " threshold2 = " << thresholds[i][1] << endl;
        findImageCircles(imageData, thresholds[i][0], thresholds[i][1], mat1, contours, subcenters);

        foreach (const KeyPoint& pt1, subcenters) {
            bool toAdd = true;
            foreach (const KeyPoint& pt2, centers) {
                if (distance(pt2, pt1) < 10) {
                    toAdd = false;
                    break;
                }
            }
            if (toAdd) {
                centers.push_back(pt1);
            }
        }

        if  (bDebug) {
            Mat draw;
            cvtColor(mat1, draw, COLOR_GRAY2BGR);
            Scalar color = Scalar(255, 0, 0);
            drawContours(draw, contours, -1, color, 3, 8);

            cv::imshow("binaryimage", draw);

            int key = cv::waitKey();
            cout << "key=" << key << endl;
        }
        if (i == thresholds.size()-1 && bDebug) {

            Mat draw;
            cvtColor(mat1, draw, COLOR_GRAY2BGR);
            Scalar color = Scalar(255, 0, 0);
            foreach(const KeyPoint &k, centers) {
                circle(draw, k.pt, k.size, color, 2, 8);
            }
            cv::imshow("binaryimage", draw);

            int key = cv::waitKey();
            cout << "key=" << key << endl;
        }
    }

    if (centers.size() > 0) {
        cout << "Number of found iron: " << centers.size() << endl;
        vector<cv::KeyPoint> innerVector, outerVector;
        if (Parse2DDicomImage::detectRuler2DPoints(centers, outerVector, innerVector, false, bDebug? imageOutputFolder: nullptr)) {
            for (auto it = innerVector.begin(); it != innerVector.end(); it++) {
                cout << "inner: " << it->pt.x << " ," << it->pt.y << endl;
                innerPts.push_back(it->pt);
            }
            for (auto it = outerVector.begin(); it != outerVector.end(); it++) {
                cout << "outer: " << it->pt.x << " ," << it->pt.y << endl;
                outerPts.push_back(it->pt);
            }
            return true;
        } else {
            cout << "Cannot dect inner and outter" << endl;
        }
    } else {
        cout << "No iron is found" << endl;
    }

    return false;
}

/// 根据投影矩阵P = A*[R t] 反向计算 A
///
void computeCameraMatrix(Matrix4d *m, double &u, double &v, double &f)
{
    //m->Print(std::cout);
    vector<double> row1 = m->getRow(0);
    vector<double> row2 = m->getRow(1);
    vector<double> row3 = m->getRow(2);


    double ry = asin(row3[0]);// atan2(-row3[0], sqrt(row3[1]*row3[1] + row3[2]*row3[2])); // ry = atan2(-a31, sqrt(a32^2+a33^2))

    double rx = 0;
    if (cos(ry) != 0) {
        rx = atan2(row3[1]/cos(ry), row3[2]/(cos(ry)));
    }

    double t3 = row3[3];

    //double u, v, f;
    double rz;

    CeresSolProjMatrix::solve(rx, ry,
            row1[0], row1[1], row1[2],
            row2[0], row2[1], row2[2],
            u, v, f, rz);

    double t1 = -u * t3/f;
    double t2 = -v * t3/f;

    cout << "u=" << u << " v=" << v << " ";
    cout << "f=" << f << " ";
    cout << "rx=" << rx * 180/M_PI << " ";
    cout << "ry=" << ry * 180/M_PI << " ";
    cout << "rz=" << rz * 180/M_PI << endl;

    cout << "t1=" << t1 << " t2=" << t2 << " t3=" << t3 << endl;

    cout << "验证结果:" << endl;
    cout << "cal R:" << endl;
    Matrix4d R = MeTransform::rotateZ(rz) *MeTransform::rotateY(ry)*MeTransform::rotateX(rx);
    R(0, 3) = t1;
    R(1, 3) = t2;
    R(2, 3) = t3;

    Matrix4d camK;
    camK.identity();
    camK(0, 0) = f;
    camK(1, 1) = f;
    camK(0, 2) = u;
    camK(1, 2) = v;

    cout << "R = \n" << R << endl;
    cout << "camK = \n" << camK << endl;
    Matrix4d newP = camK * R;
    cout << "newP = \n" << newP << endl;
}

int main(int argc, char *argv[])
{
    if (argc < 3) {
        std::cerr << "Usage: " << std::endl;
        std::cerr
                << argv[0]
                << " option inputDicomFile"
                << std::endl;
        std::cerr << "option: 1 - show binary image with dynamic adjust threshold. \n"
                  << "    2 - 读取文件中的数据，生成投影矩阵P，然后求解相机参数\n"
                  << std::endl;
        return EXIT_FAILURE;
    }

    int option = atoi(argv[1]);
    string inputDicomFile = argv[2];
    QString filename = QString::fromStdString(inputDicomFile);
    QString imageOutputFolder = "./irondect";

    if (option == 1) {
        readDicomToBinaryImage(inputDicomFile, true);
    } else if (option == 2) {
        vector<cv::Point2f> innerVector, outerVector;
        findDicomIron(inputDicomFile, &imageOutputFolder, innerVector, outerVector, false);

        vector<Eigen::Vector3d> ptsRuler2DIronBallTop_W = {
            {33.50, 73.77, 78},
            {-3.27, 37, 78},
            {33.50, 0.23, 78},
            {70.27, 37, 78},
            {33.50, 63.77, 78},
        };
        vector<Eigen::Vector3d> ptsRuler2DIronBallBot_W = {
            {67, 7, 2},
            {0, 0, 2},
            {0, 74, 2},
            {67, 67, 2},
            {71.95, 2.05, 2},
            {55.06, 5.75, 2},
        };
        Matrix4d matrixP;
        CeresSolProjMatrix::calProjectionMatrix_UVfrW(ptsRuler2DIronBallTop_W, ptsRuler2DIronBallBot_W, innerVector, outerVector, matrixP);
        cout << "matrixP = " << matrixP << endl;

        cout << "verify P:" << endl;
        foreach(const Eigen::Vector3d &p, ptsRuler2DIronBallBot_W) {
            Eigen::Vector3d inP = p;
            Eigen::Vector3d outP;
            matrixP.multiplyPoint3D(inP.data(), outP.data());
            cout << "outP:" << outP[0]/outP[2] << ", " << outP[1]/outP[2] << endl;
        }
        double u, v, f;
        computeCameraMatrix(&matrixP, u, v, f);
    }

    return 0;
}
